import ghhops_server as hs
import rhino3dm as rg
from rtde_receive import RTDEReceiveInterface
from flask import Flask, request

import random
import math

from utils.z_theta_and_Rxyz import Rxyz_to_theta

app = Flask(__name__)
hops = hs.Hops(app)


class MyRTDEReceiver:
    def __init__(self):
        self.rtde_r = RTDEReceiveInterface("192.168.111.130")
        print("RTDE Receiver is ready")

    def get_joint_angles(self):
        return [
            i / math.pi * 180 for i in self.rtde_r.getActualQ()
        ]

    def get_tcp_pose(self):
        data = self.rtde_r.getActualTCPPose()
        return [
            i * 1000 for i in data[:3]
        ], data[3:]


my_receiver = MyRTDEReceiver()

@hops.component(
        "/get_joint_angles",
    name="get_joint_angles",
    description="get_joint_angles",
    inputs=[],
    outputs=[
        hs.HopsNumber("joint_angles", "joint_angles"),
    ]
)
def get_joint_angles():
    return my_receiver.get_joint_angles()


@hops.component(
        "/get_tcp_pose",
    name="get_tcp_pose",
    description="get_tcp_pose",
    inputs=[],
    outputs=[
        hs.HopsPoint("TCP position", "TCP position"),
        hs.HopsNumber("TCP orientation", "TCP orientation"),
        hs.HopsNumber("TCP Z Angle", "TCP Z Angle"),
    ]
)
def get_tcp_pose():
    position, orientation = my_receiver.get_tcp_pose()
    point = rg.Point3d(position[0], position[1], position[2])
    z_angle = Rxyz_to_theta(orientation[0], orientation[1], orientation[2])
    return point, orientation, z_angle


if __name__ == '__main__':
    app.run(debug=True, port=5000)
